
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.templates.commands.*;
import edu.wpi.first.wpilibj.templates.subsystems.DifferentialSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.PIDMainArmSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.PIDSecondaryArmSubsystem;

public class OI {
    public static final int kLogitechDriverPort = 1;
    public static final int kLogitechCoDriverPort = 2;
    public static final int kLogitechCalibrationPort = 3;

    
    private LogitechController driverController;
    private LogitechControllerButton driverButtonA;
    private LogitechControllerButton driverButtonB;
    private LogitechControllerButton driverButtonX;
    private LogitechControllerButton driverButtonY;
    private LogitechControllerButton driverLeftBumper;
    private LogitechControllerButton driverRightBumper;
    private LogitechControllerButton driverStart;
    private LogitechControllerButton driverBack;
    private LogitechControllerButton driverLeftTrigger;
    private LogitechControllerButton driverRightTrigger;
    
    private LogitechController coDriverController;
    private LogitechControllerButton coDriverButtonA;
    private LogitechControllerButton coDriverButtonB;
    private LogitechControllerButton coDriverButtonX;
    private LogitechControllerButton coDriverButtonY;
    private LogitechControllerButton coDriverLeftBumper;
    private LogitechControllerButton coDriverRightBumper;
    private LogitechControllerButton coDriverStart;
    private LogitechControllerButton coDriverBack;
    private LogitechControllerButton coDriverLeftTrigger;
    private LogitechControllerButton coDriverRightTrigger;
    private LogitechControllerButton coDriverUpGamepad;
    private LogitechControllerButton coDriverDownGamepad;
    private LogitechControllerButton coDriverLeftGamepad;
    private LogitechControllerButton coDriverRightGamepad;

    private LogitechController m_CalibrationGamePad;
    private LogitechControllerButton m_setCalibratorLeftShoulderButton;
    private LogitechControllerButton m_setCalibratorLeftTriggerButton;
    private LogitechControllerButton m_setCalibratorRightShoulderButton;
    private LogitechControllerButton m_setCalibratorRightTriggerButton;
    
    /*
     * Constructor
     */
    
    public OI() {
       
        /*
         * Driver Controls 
         */
        driverButtonA = new LogitechControllerButton(driverController, 
                LogitechController.ButtonType.kButtonA.value);
        
        driverButtonB = new LogitechControllerButton(driverController, 
                LogitechController.ButtonType.kButtonB.value);
        
        driverButtonX = new LogitechControllerButton(driverController, 
                LogitechController.ButtonType.kButtonX.value);
        
        driverButtonY = new LogitechControllerButton(driverController, 
                LogitechController.ButtonType.kButtonY.value);
        
        driverLeftBumper = new LogitechControllerButton(driverController, 
                LogitechController.ButtonType.kButtonLeftB.value);
        
        driverRightBumper = new LogitechControllerButton(driverController, 
                LogitechController.ButtonType.kButtonRightB.value);
        
        driverStart = new LogitechControllerButton(driverController, 
                LogitechController.ButtonType.kButtonStart.value);
        
        driverBack = new LogitechControllerButton(driverController, 
                LogitechController.ButtonType.kButtonBack.value);
        
        driverLeftTrigger = new LogitechControllerButton(driverController, 
                LogitechController.ButtonType.kButtonLeftTrigger.value);
        
        driverRightTrigger = new LogitechControllerButton(driverController, 
                LogitechController.ButtonType.kButtonRightTrigger.value);
        
        /*
         * Driver Command Layout
         */
        
        driverLeftBumper.whenPressed(
                new ShiftDifferential(DifferentialSubsystem.DifferentialPosition.LOW_GEAR));
        
        driverRightBumper.whenPressed(
                new ShiftDifferential(DifferentialSubsystem.DifferentialPosition.LOW_GEAR));
        
        /*
         * Co-Driver controls
         */
        
        coDriverButtonA = new LogitechControllerButton(coDriverController, 
                LogitechController.ButtonType.kButtonA.value);
        
        coDriverButtonB = new LogitechControllerButton(coDriverController, 
                LogitechController.ButtonType.kButtonB.value);
        
        coDriverButtonX = new LogitechControllerButton(coDriverController, 
                LogitechController.ButtonType.kButtonX.value);
        
        coDriverButtonY = new LogitechControllerButton(coDriverController, 
                LogitechController.ButtonType.kButtonY.value);
        
        coDriverLeftBumper = new LogitechControllerButton(coDriverController, 
                LogitechController.ButtonType.kButtonLeftB.value);
        
        coDriverRightBumper = new LogitechControllerButton(coDriverController, 
                LogitechController.ButtonType.kButtonRightB.value);
        
        coDriverStart = new LogitechControllerButton(coDriverController, 
                LogitechController.ButtonType.kButtonStart.value);
        
        coDriverBack = new LogitechControllerButton(coDriverController, 
                LogitechController.ButtonType.kButtonBack.value);
        
        coDriverLeftTrigger = new LogitechControllerButton(coDriverController, 
                LogitechController.ButtonType.kButtonLeftTrigger.value);
        
        coDriverRightTrigger = new LogitechControllerButton(coDriverController, 
                LogitechController.ButtonType.kButtonRightTrigger.value);
        
        coDriverUpGamepad = new LogitechControllerButton(coDriverController,
                LogitechController.ButtonType.kButtonGameUp.value);
        
        coDriverDownGamepad = new LogitechControllerButton(coDriverController,
                LogitechController.ButtonType.kButtonGameDown.value);
        
        coDriverLeftGamepad = new LogitechControllerButton(coDriverController,
                LogitechController.ButtonType.kButtonGameLeft.value);
        
        coDriverRightGamepad = new LogitechControllerButton(coDriverController,
                LogitechController.ButtonType.kButtonGameRight.value);
        
        /*
         * Co-Driver command Layout
         */
        
        // Main Arm Controls
        coDriverButtonA.whenPressed(
                new SetMainArmPosition(PIDMainArmSubsystem.MainArmPosition.MAIN_ARM_FULLY_LOWERED));
        
        coDriverButtonB.whenPressed(
                new SetMainArmPosition(PIDMainArmSubsystem.MainArmPosition.MAIN_ARM_INTERMEDIATE_B));
        
        coDriverButtonX.whenPressed(
                new SetMainArmPosition(PIDMainArmSubsystem.MainArmPosition.MAIN_ARM_INTERMEDIATE_A));
        
        coDriverButtonY.whenPressed(
                new SetMainArmPosition(PIDMainArmSubsystem.MainArmPosition.MAIN_ARM_EXTENDED));
        
        // Secondary Arm Controls
        coDriverUpGamepad.whenPressed(
                new SetSecondaryArmPosition(PIDSecondaryArmSubsystem.SecondaryArmPosition.NINETY_ABOVE_LEVEL));
        
        coDriverDownGamepad.whenPressed(
                new SetSecondaryArmPosition(PIDSecondaryArmSubsystem.SecondaryArmPosition.NINTEY_BELOW_LEVEL));
        
        coDriverLeftGamepad.whenPressed(
                new SetSecondaryArmPosition(PIDSecondaryArmSubsystem.SecondaryArmPosition.FORTY_FIVE_BELOW_LEVEL));
        
        coDriverRightGamepad.whenPressed(
                new SetSecondaryArmPosition(PIDSecondaryArmSubsystem.SecondaryArmPosition.FORTY_FIVE_ABOVE_LEVEL));
        
        coDriverLeftBumper.whenPressed(
                new SetSecondaryArmPosition(PIDSecondaryArmSubsystem.SecondaryArmPosition.LEVEL_WITH_ARM));
        
    } // end IO Constructor      
    
    /**
     * Grabs the first controller plugged into the laptop.
     * @return The Driver Controller.
     */
    
    public LogitechController getDriverController(){
        return driverController;
    } // end getDriverController
    
    /**
     * Grabs the second controller plugged into the laptop.
     * @return The Co-Driver controller.
     */
    
    public LogitechController getCoDriverController(){
        return coDriverController;
    } // end getCoDriverController
    
    // Unused for now.
    public LogitechController getCalibrationController(){
        return m_CalibrationGamePad;
    } // end getCalibrationController 
    
} // end IO

